#ifndef __JOINTIO_H__
#define __JOINTIO_H__

#include<vector>
#include "daqAdapter.h"

//manage DLLEXPORT FLAG
#ifdef _WIN32
		#ifdef jointIO_EXPORTS
			#define DLLEXPORT __declspec(dllexport)
		#else
			#define DLLEXPORT __declspec(dllimport)
		#endif
#else
		#define DLLEXPORT
#endif

// C functions export
#ifdef __cplusplus
extern "C"{
#endif
//start C functions export
	typedef void* JointIOHandle;
	DLLEXPORT int JointIO_getNumJoint(JointIOHandle* hjio);
	DLLEXPORT void JointIO_create(JointIOHandle* hjio);
	DLLEXPORT void JointIO_destroy(JointIOHandle hjio);
	DLLEXPORT void JointIO_readJoints(JointIOHandle hjio, double *pot, double *psA, double* psB, double* extra, const int sz);
	DLLEXPORT void JointIO_writeJoints(JointIOHandle hjio,const double *  torqueA, const double*  torqueB, int sz);
	DLLEXPORT void JointIO_writeValves(JointIOHandle hjio, double *buffer, int szBuffer);
	DLLEXPORT void JointIO_read(JointIOHandle hjio, double* buffer);
	//need to connect these names to the URDF model


//endof C functions export
#ifdef __cplusplus
};
#endif

//high-level joint position read and torque apply
//including sensor/actuator remapping

//implements serializable
class DLLEXPORT MapTable
{
protected:
	double bias;
public:
	inline MapTable()
	{
		;
	}
	inline double getMappedValue(int ch, double inValue)
	{
		return inValue;
	}
};

//EnumId = IdInDocs -1;
const enum {
	/* Head */
	J_HEAD_ROTATE, J_UPPER_NECK_BACKFORTH, J_UPPER_NECK_RIGHTLEFT,J_LOWER_NECK_BACKFORTH,
	/* torso */
	J_CHEST_BACKFORTH /*4*/, J_WAIST_ROTATE, J_WAIST_GIMBAL_RIGHT, J_WAIST_GIMBAL_LEFT,
	/*LEFT shoulder - arm */
	J_LEFT_SHOULDER_BACKFORTH, J_LEFT_SHOULDER_UPDOWN, 
	J_LEFT_ARM_UPDOWN, J_LEFT_ARM_OPENCLOSE, J_LEFT_UPPERARM_ROTATE, J_LEFT_ELBOW_ROTATE, 
	J_LEFT_FOREARM_RORATE, J_LEFT_WRIST,
	/* RIGHT shoulder-arm */
	J_RIGHT_SHOULDER_BACKFORTH, J_RIGHT_SHOULDER_UPDOWN, 
	J_RIGHT_ARM_UPDOWN, J_RIGHT_ARM_OPENCLOSE, J_RIGHT_UPPERARM_ROTATE, J_RIGHT_ELBOW_ROTATE, 
	J_RIGHT_FOREARM_RORATE, J_RIGHT_WRIST,
	/*LEFT LEG*/
	J_LEFT_HIP_OPENCLOSE, J_LEFT_HIP_BACKFORTH, J_LEFT_HIP_RORATE, 
	J_LEFT_KNEE, J_LEFT_ANKLE_ROTATE, J_LEFT_ANKLE_OUTER, J_LEFT_ANKLE_INNER,
	/*RIGHT LEG*/
	J_RIGHT_HIP_OPENCLOSE, J_RIGHT_HIP_BACKFORTH, J_RIGHT_HIP_RORATE, 
	J_RIGHT_KNEE, J_RIGHT_ANKLE_ROTATE, J_RIGHT_ANKLE_OUTER, J_RIGHT_ANKLE_INNER,
	/* fingers */
	J_LEFT_THUMB, J_LEFT_INDEX, J_LEFT_PALM,
	J_RIGHT_THUMB, J_RIGHT_INDEX, J_RIGHT_PALM
};

static	const char * jointNames[] = {
	"J_HEAD_ROTATE", "J_UPPER_NECK_BACKFORTH", "J_UPPER_NECK_RIGHTLEFT","J_LOWER_NECK_BACKFORTH",
	"J_CHEST_BACKFORTH ", "J_WAIST_ROTATE", "J_WAIST_GIMBAL_RIGHT", "J_WAIST_GIMBAL_LEFT",
	/*LEFT shoulder - arm */
	"J_LEFT_SHOULDER_BACKFORTH", "J_LEFT_SHOULDER_UPDOWN", 
	"J_LEFT_ARM_UPDOWN", "J_LEFT_ARM_OPENCLOSE", "J_LEFT_UPPERARM_ROTATE", "J_LEFT_ELBOW_ROTATE", 
	"J_LEFT_FOREARM_RORATE", "J_LEFT_WRIST",
	/* RIGHT shoulder-arm */
	"J_RIGHT_SHOULDER_BACKFORTH", "J_RIGHT_SHOULDER_UPDOWN", 
	"J_RIGHT_ARM_UPDOWN", "J_RIGHT_ARM_OPENCLOSE", "J_RIGHT_UPPERARM_ROTATE", "J_RIGHT_ELBOW_ROTATE", 
	"J_RIGHT_FOREARM_RORATE", "J_RIGHT_WRIST",
	/*LEFT LEG*/
	"J_LEFT_HIP_OPENCLOSE", "J_LEFT_HIP_BACKFORTH", "J_LEFT_HIP_RORATE", 
	"J_LEFT_KNEE", "J_LEFT_ANKLE_ROTATE", "J_LEFT_ANKLE_OUTER", "J_LEFT_ANKLE_INNER",
	/*RIGHT LEG*/
	"J_RIGHT_HIP_OPENCLOSE", "J_RIGHT_HIP_BACKFORTH", "J_RIGHT_HIP_RORATE", 
	"J_RIGHT_KNEE", "J_RIGHT_ANKLE_ROTATE", "J_RIGHT_ANKLE_OUTER", "J_RIGHT_ANKLE_INNER",
	/* fingers */
	"J_LEFT_THUMB", "J_LEFT_INDEX", "J_LEFT_PALM",
	"J_RIGHT_THUMB", "J_RIGHT_INDEX", "J_RIGHT_PALM"
};


class DLLEXPORT JointIO
{
public:
	int numJoint; 
	JointIO(MapTable* _readPSMap=NULL, MapTable* _readPOTMap=NULL, MapTable* _writeMap=NULL);	
	int writeJoints(const double* torqueA, const double* torqueB, const int sz);
	int writeValves( double * buffer, int szBuffer);
	int readJoints( double *pot=NULL, double *psA=NULL, double* psB=NULL, double* extra=NULL, const int sz=0);
	int read(double* buffer);
	~JointIO();
protected:
	MapTable *readPSMap, *readPOTMap, *writeMap;
	DaqIO *daqWrite, *daqRead;
};



#endif
